﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Beetle.DimaHead;
using Beetle.FandorinsBugBrain;


namespace Beetle
{
    public class Position
    {
        public double x;
        public double y;
        private double viewAngle;
        public double ViewAngle
        {
            get { return viewAngle; }
            set
            {
                if (value >= 360) viewAngle = value - 360;
                else
                {
                    if (value < 0) viewAngle = 360 + value;
                    else viewAngle = value;
                }
            }
        }

        public Position(double tx, double ty)
        {
            x = tx;
            y = ty;
            viewAngle = 0;
        }

        public Position()
        {
            x = 0;
            y = 0;
            viewAngle = 0;
        }
    }

    public class Bettle
    {
        public class Leg
        {
            public int Side; //К какой стороне относится 0-левая, 1 - правая
            public int IsUp = 0; //Поднята ли нога 0 - нет, 1 - да
            private int state = 1;

            public int State //0,1,2 позиции ноги, от попы к голове
            {
                get
                {
                    return state;
                }
                set
                {
                    if (value > 2) state = 2;
                    else if (value < 0) state = 0;
                    else state = value;
                }
            }

        }

        #region Коэфициенты

        private double MoveCoef = 1; //Коэф перемещения
        private double Trenie = 5; //Коэф трения пузом
        private double AngleKoef = 15; //Коэф поворота
        private double ZadomKoef = 0; //Коэф движения задом

        #endregion

        public Position position; //Позиция таракана

        public Position previousCoord;

        public Leg[] Legs;

        public Brain Brain;

        #region Интрфейс

        public void MakeStep(Target[] targets)
        {
            Move(Brain.MakeStep(this, targets));
        }

        public void MakeStepWithLearning(Target[] targets)
        {
            Move(Brain.MakeStepWithLearning(this, targets));
        }

        public double CaclcError()
        {
            return Brain.CalcError();
        }

        public List<double> ErrorList()
        {
            return Brain.ErrorList;
        }

        #endregion

        #region Логика

        /// <summary>
        /// this method count new coordinates for the new state of legs(and move the bug)
        /// </summary>
        /// <param name="newLegs">new state of legs</param>
        /// <returns>new coordinates</returns>
        public void Move(Leg[] newLegs)
        {
            previousCoord = position;
            position = HypMove(newLegs);
            Legs = newLegs;
        }

        /// <summary>
        /// this method count new coordinates for the new state of legs(not move the bug)
        /// </summary>
        /// <param name="newLegs">new state of legs</param>
        /// <returns>new coordinates</returns>
        public Position HypMove(Leg[] newLegs)
        {
            Position tempPosition = new Position();

            Leg[] NewLeftLegs = newLegs.Where(l => l.Side == 0).ToArray();
            Leg[] NewRightLegs = newLegs.Where(l => l.Side == 1).ToArray();

            Leg[] LeftLegs = Legs.Where(l => l.Side == 0).ToArray();
            Leg[] RightLegs = Legs.Where(l => l.Side == 1).ToArray();

            int DownLegsCount = 0;

            int LeftSideStep = 0;
            int RightSideStep = 0;

            for (int i = 0; i < LeftLegs.Count(); i++)
            {
                if (LeftLegs[i].IsUp == 0 && NewLeftLegs[i].IsUp == 0)
                {
                    LeftSideStep += LeftLegs[i].State - NewLeftLegs[i].State;
                    DownLegsCount++;
                }
            }

            for (int i = 0; i < RightLegs.Count(); i++)
            {
                if (RightLegs[i].IsUp == 0 && NewRightLegs[i].IsUp == 0)
                {
                    RightSideStep += RightLegs[i].State - NewRightLegs[i].State;
                    DownLegsCount++;
                }
            }

            double localMoveCoef = MoveCoef;
            if (DownLegsCount < 3) localMoveCoef *= Trenie;
            if ((LeftSideStep + RightSideStep) < 0) localMoveCoef *= ZadomKoef;

            int dAngle = RightSideStep - LeftSideStep;

            tempPosition.ViewAngle = position.ViewAngle + dAngle * AngleKoef;
            tempPosition.x = position.x;
            tempPosition.y = position.y;

            double Distanse = (RightSideStep + LeftSideStep) * localMoveCoef;

            if (RightSideStep > 0 && LeftSideStep > 0)
            {

            }

            if ((RightSideStep * LeftSideStep) > 0)
            {
                if (tempPosition.ViewAngle >= 0 && tempPosition.ViewAngle <= 90)
                {
                    tempPosition.x += Math.Cos(position.ViewAngle * (Math.PI / 180)) * Distanse;
                    tempPosition.y -= Math.Sin(position.ViewAngle * (Math.PI / 180)) * Distanse;
                }
                else
                {
                    if (tempPosition.ViewAngle >= 90 && tempPosition.ViewAngle <= 180)
                    {
                        tempPosition.x -= Math.Cos((180 - position.ViewAngle) * (Math.PI / 180)) * Distanse;
                        tempPosition.y -= Math.Sin((180 - position.ViewAngle) * (Math.PI / 180)) * Distanse;
                    }
                    else
                    {

                        if (tempPosition.ViewAngle >= 180 && tempPosition.ViewAngle <= 270)
                        {
                            tempPosition.x -= Math.Cos((position.ViewAngle - 180) * (Math.PI / 180)) * Distanse;
                            tempPosition.y += Math.Sin((position.ViewAngle - 180) * (Math.PI / 180)) * Distanse;
                        }
                        else
                        {

                            if (tempPosition.ViewAngle >= 270 && tempPosition.ViewAngle <= 360)
                            {
                                tempPosition.x += Math.Cos((360 - position.ViewAngle) * (Math.PI / 180)) * Distanse;
                                tempPosition.y += Math.Sin((360 - position.ViewAngle) * (Math.PI / 180)) * Distanse;
                            }
                        }
                    }
                }
            }
            return tempPosition;
        }

        public double AngleToTarget(Target target) //Найти угол между взглядом и целю полный отщет от оси х
        {
            double A1 = 1;
            double B1 = 0;
            double A2 = target.coord.x - position.x;
            double B2 = target.coord.y - position.y;

            double result = Math.Acos(Math.Abs((A1 * A2 + B1 * B2) / (Math.Sqrt(Math.Pow(A1, 2) + Math.Pow(B1, 2)) * Math.Sqrt(Math.Pow(A2, 2) + Math.Pow(B2, 2))))) * (180 / Math.PI);

            if (target.coord.x < position.x) result = result + 90;
            if (target.coord.y > position.y) result = 360 - result;

            result = result - position.ViewAngle;

            if (result >= 360) result = result - 360;
            if (result < 0) result = 360 + result;

            return result;
        }

        public double DistanseToTarget(Target target) //Найти растояние до цели
        {
            return Math.Sqrt(Math.Pow(target.coord.x - position.x, 2) + Math.Pow(target.coord.y - position.y, 2));
        }

        #endregion

        #region Конструкторы

        public Bettle(string Owner)
        {
            if (Owner == "foxiz")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle = 15 };

                Brain = new DimaBrain();
            }

            if (Owner == "fandorin")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle=15};
                previousCoord = position;
                Brain = new FBrain();
            }
        }

        public Bettle(string Owner,string NeuroFileName)
        {
            if (Owner == "foxiz")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle = 15 };

                Brain = new DimaBrain(NeuroFileName);
            }

            if (Owner == "fandorin")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle = 15 };
                previousCoord = position;
                Brain = new FBrain();
            }
        }

        public Bettle(string Owner, string NeuroFileName,string ErrorFileName)
        {
            if (Owner == "foxiz")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle = 15 };

                Brain = new DimaBrain(NeuroFileName, ErrorFileName);
            }

            if (Owner == "fandorin")
            {
                Legs = new Leg[6];
                for (int i = 0; i < 3; i++) Legs[i] = new Leg { Side = 0 };
                for (int i = 3; i < 6; i++) Legs[i] = new Leg { Side = 1 };

                position = new Position(300, 300) { ViewAngle = 175 };
                previousCoord = position;
                Brain = new FBrain(NeuroFileName, ErrorFileName);
            }
        }

        #endregion
    }
}
